import lcm
import sys
import os
import time
from threading import Thread, Lock

from robot_control_cmd_lcmt import robot_control_cmd_lcmt
from robot_control_response_lcmt import robot_control_response_lcmt
from localization_lcmt import localization_lcmt

import toml
import copy
import math
from file_send_lcmt import file_send_lcmt
class Vec3:  
    def __init__(self, x=0, y=0, z=0):  
        self.x = x  
        self.y = y  
        self.z = z  
  
    def __getitem__(self, key):  
        if key == 0:  
            return self.x  
        elif key == 1:  
            return self.y  
        elif key == 2:  
            return self.z  
        else:  
            raise IndexError("Vec3 index out of range")  
  
    def __setitem__(self, key, value):  
        if key == 0:  
            self.x = value  
        elif key == 1:  
            self.y = value  
        elif key == 2:  
            self.z = value  
        else:  
            raise IndexError("Vec3 index out of range")  
  
class Motor_Ctrl(object):
    def InverseKinematic(quad, leg, p):  
        # 假设quad是一个Quadruped实例，包含必要的属性  
        # p是一个Vec3实例或任何可以通过索引访问x, y, z的类似对象  
    
        l1 = quad.abad_link_length_  
        l2 = quad.hip_link_length_  
        l3 = quad.knee_link_length_  
        l4 = quad.knee_link_y_offset_  
    
        x, y, z = p.x, p.y, p.z  
    
        if abs(y) < 0.0001:  
            y = 0.0001  
    
        ss1 = math.sqrt(z**2 + y**2)  
    
        side_sign = quad.GetSideSign(leg)  # 假设Quadruped类有一个GetSideSign方法  
    
        if side_sign * y >= 0:  
            q_des_0 = side_sign * (math.acos((z**2 + ss1**2 - y**2) / (2 * abs(z) * ss1)) +  
                                   math.acos((l1 + l4) / ss1) - math.pi / 2)  
        else:  
            q_des_0 = side_sign * (-math.acos((z**2 + ss1**2 - y**2) / (2 * abs(z) * ss1)) +  
                                   math.acos((l1 + l4) / ss1) - math.pi / 2)  
    
        ss2 = math.sqrt(ss1**2 - (l1 + l4)**2 + x**2)  
    
        q_des_2 = math.acos((l2**2 + l3**2 - ss2**2) / (2 * l2 * l3))  
        q_des_2 = math.pi - q_des_2  
    
        if z <= 0:  
            q_des_1 = -math.acos((l2**2 + ss2**2 - l3**2) / (2 * l2 * ss2)) + math.asin(x / ss2)  
        else:  
            q_des_1 = -math.acos((l2**2 + ss2**2 - l3**2) / (2 * l2 * ss2)) + (math.pi - math.asin(x / ss2))  
    
        # 检查NaN值（虽然Python的math函数通常不会直接返回NaN，但这里为了完整性还是检查）  
        if math.isnan(q_des_0) or math.isnan(q_des_1) or math.isnan(q_des_2):  
            # print(f"[LegController] leg {leg} IK result contains NaN!")  
            return None  # 或者可以选择抛出异常  
    
        return Vec3(q_des_0, q_des_1, q_des_2)  
    
    # 注意：这里需要Quadruped类的定义，以及它包含必要的属性和方法，如abad_link_length_, hip_link_length_等  
    # 以及GetSideSign(leg)方法